#include "bsp_NUC.h"
#include "bsp_navigation.h"
#include "bsp_navigation_RPM.h"
//#include "bsp_wheel.h"
#include "bsp_remote.h"
#include "bsp_can.h"



struct wheel_info RPM_info;
struct _RPM RPM_pid={5,0.5,0};
static short r0,f0,y0;
static short y0_box;

void mps_to_RPM(void)
{
	//static short r0,f0,y0;
	f0=cmd_vel.cmd_vel[0]/0.15/3.14159f*60;
	r0=cmd_vel.cmd_vel[1]/0.15/3.14159f*60;
	if(!cmd_vel.cmd_vel[2])
	{
		y0=y0_box;
	}
	y0=cmd_vel.cmd_vel[2]*R/0.15/3.14159f*60;
}

void RPM_calculation(void)
{
	RPM_info.RPM_target[0]=(-f0+r0+y0)*kk;
	RPM_info.RPM_target[1]=(f0+r0+y0)*kk;
	RPM_info.RPM_target[2]=(f0-r0+y0)*kk;
	RPM_info.RPM_target[3]=(-f0-r0+y0)*kk;
}


void RPM_close(void)
{
	static short delta=0;
	static short p_part,i_part,pid_out;
	int i=0;
	float Interval[4];
	for(i=0;i<4;i++)
	{
		delta=RPM_info.RPM_target[i]-chasis_3508[i].feedback_speed;
		p_part=RPM_pid.p*delta;
		Interval[i]+=i_part*delta;
		i_part=Interval[i];
		if(p_part>RPM_limit) p_part=RPM_limit;
		if(p_part<-RPM_limit) p_part=-RPM_limit;
		i_part=RPM_pid.i*delta;
		if(i_part>600) i_part=600;
		if(i_part<-600) i_part=-600;
		pid_out=p_part+i_part;
		chasis_3508[i].motor_out=pid_out;
	}
	
}

void navigation_RPM(void)
{
	static struct Chassis_Angle_Info chassis_angle_info;
	//static short y0_box;
  chassis_angle_info=Target_and_Feedback();             
	//y0_box=Outer_Loop(chassis_angle_info); 
	mps_to_RPM();
	RPM_calculation();
	RPM_close();
	CAN_TRANSMIT_Wheel();
}









